/**

This file is part of MaCI/GIMnet.

MaCI/GIMnet is free software: you can redistribute it and/or modify it 
under the terms of the GNU Lesser General Public License as published 
by the Free Software Foundation, either version 3 of the License, or 
(at your option) any later version.

MaCI/GIMnet is distributed in the hope that it will be useful, but WITHOUT 
ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 
FITNESS FOR A PARTICULAR PURPOSE.  See the GNU Lesser General Public 
License for more details.

You should have received a copy of the GNU Lesser General Public 
License along with GIMnet. (See COPYING.LESSER) If not, see 
<http://www.gnu.org/licenses/>.

**/
####### NOTE : THIS IS NOT WORKING YET VERY WELL
####### STILL UNDER DEVELOPMENT

# example client for map client with swig wrapped c++ library in python
import sys, time, array
sys.path.append("../../../interfaces/wrap/Python/lib")
sys.path.append("../../../../GIMI/wrap/Python/lib")


import MapClient as MC
import gimi;

if sys.argv.__len__() != 4:
    print "Usage: python %s hubAddress hubPort MapClientMaCISL"%sys.argv[0]
    sys.exit()

hubAddress = sys.argv[1]
hubPort = sys.argv[2]
mapClientMaCISL = sys.argv[3]


g = gimi.GIMI()
mapClient = MC.CMapClient(g,0)

##Connect to gimi
print "connecting to ", hubAddress,":",hubPort
ret = g.connectToHubEx(str(hubAddress),int(hubPort),"")
print MC.GetErrorStr(ret)


##then connect map client
print "Setting default target: '",mapClientMaCISL,"' to MapClient"
mapClient.SetDefaultTarget(MC.SMaCISL(mapClientMaCISL))

##Open map client

print "Opening map client...."
ret = mapClient.Open();
if ret == MC.KMaCIOK:
    print "Succesfully opened map client"
else:
    print "Failed to open map client"



print "asking for map..."

mapData = MC.CMapData()

timeout_ms = 2000
if mapClient.GetMap(mapData,timeout_ms) == True:
    print "GOT MAP!"
    mapHeader = mapData.GetMapHeader()
    print "Resolution is ",mapHeader.gridDataResolution

    gridBlockHeader = mapData.GetGridBlockHeader(0)
    print "Data origin (",gridBlockHeader.dataOriginX,",",gridBlockHeader.dataOriginY,") size:",gridBlockHeader.dataSize," . Type:", gridBlockHeader.dataType," . Position (",gridBlockHeader.x,",",gridBlockHeader.y,")" 
		
    print "Grid block count : ",mapData.GetGridBlockCount()
    kartta = mapData.GetGridBlockData(1)
		if gridBlockHeader.dataType == MC.KGridTypeOccupancy:
    	print "block data:", array.array('B', kartta)
		if gridBlockHeader.dataType == MC.KGridTypeHeightMap:
    	print "block data:", array.array('f', kartta)
else:
    print "Failed to get the map"

print "closing"

mapClient.Close()   


import sys, time
sys.path.append("../../../interfaces/wrap/Python/lib")
sys.path.append("../../../../GIMI/wrap/Python/lib")
import MapClient as MC
import gimi
g = gimi.GIMI()
mapClient = MC.CMapClient(g,0)
ret = g.connectToHubEx("asrobo.hut.fi",40002,"")
print MC.GetErrorStr(ret)
mapClient.SetDefaultTarget(MC.SMaCISL("Test.MaCI_Map.Dummy"))
ret = mapClient.Open();
if ret == MC.KMaCIOK:
    print "Succesfully opened map client"
else:
    print "Failed to open map client"

mapData = MC.CMapData()


